/***************************************************************************
 创建者: 林逢达
 开始时间: 2016.11.28
 copyright: (C) 同川科技
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 (1)2017.01.19. 林逢达 添加TaitBryan"xyz"角正反解，并替换文档Euler"ZYZ"角
 (2)2017.02.20. 林逢达 不依赖工具与用户坐标系列表，保障线程安全
 ***************************************************************************
 *  @file coordinate.cpp
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#include "coordinate.h"
#include <kdl/frames_io.hpp>
#include <Eigen/Dense>


#define BETA
#define IF_DEBUG true

using namespace std;
using namespace KDL;

Coordinate::Coordinate(int _robotType, int _internalNum, int _robotId, QVector<JointAttribute> *_robotAttribute):
    robotType(_robotType),
    internalNum(_internalNum),
    robotId(_robotId),
    robotJoints(_robotAttribute)
{
    setComponentVersion( "Coordinate", "2017-2-20", "1.0.1" );

    //debug
    dataRecord = new DataRecord("sixR.csv", 1, 1e5);

    initRobotChain();
    jointsMax.resize(internalNum);
    jointsMin.resize(internalNum);
    for (int i=0; i<internalNum; i++)
    {
        if( (*robotJoints)[i].jointType == 0 )//旋转关节
        {
            jointsMax(i) = (*robotJoints)[i].maxPosition * M_PI / 180.0;
            jointsMin(i) = (*robotJoints)[i].minPosition * M_PI / 180.0;
        }
        else//位移关节
        {
            jointsMax(i) = (*robotJoints)[i].maxPosition / 1000.0;
            jointsMin(i) = (*robotJoints)[i].minPosition / 1000.0;
        }
    }//end for (int i=0; i<jointsNum; i++)
    fkSolverPos = new ChainFkSolverPos_recursive(robotChain);
    fkSolverVel = new ChainFkSolverVel_recursive(robotChain);
    ikSolverVel_pinv = new ChainIkSolverVel_pinv(robotChain);
    ikSolverPos_LMA = new ChainIkSolverPos_LMA(robotChain);
    ikSolverPos_NR = new ChainIkSolverPos_NR(robotChain, *fkSolverPos, *ikSolverVel_pinv,100, 1e-4);//2017.1.4修改默认迭代次数和误差
    ikSolverPos_NR_JL = new ChainIkSolverPos_NR_JL(robotChain,  jointsMin,  jointsMax, *fkSolverPos, *ikSolverVel_pinv);
}

Coordinate::~Coordinate()
{
    if(NULL!=fkSolverPos){
        delete fkSolverPos;
    }
    if(NULL!=fkSolverVel){
        delete fkSolverVel;
    }
    if(NULL!=ikSolverVel_pinv){
        delete ikSolverVel_pinv;
    }
    if(NULL!=ikSolverPos_LMA){
        delete ikSolverPos_LMA;
    }
    if(NULL!=ikSolverPos_NR){
        delete ikSolverPos_NR;
    }
    if(NULL!=ikSolverPos_NR_JL){
        delete ikSolverPos_NR_JL;
    }
}

void Coordinate::initRobotChain()
{
    Frame dhFrame;
    double a,alpha,d,theta;
    for(int i=0; i<internalNum; i++)
    {
        a = (*robotJoints)[i].dhParameter[0];
        alpha = (*robotJoints)[i].dhParameter[1];
        d = (*robotJoints)[i].dhParameter[2];
        theta = (*robotJoints)[i].dhParameter[3];
        RigidBodyInertia rigidBodyInertia=RigidBodyInertia((*robotJoints)[i].mass,
                                                            Vector((*robotJoints)[i].massCenter[0], (*robotJoints)[i].massCenter[1], (*robotJoints)[i].massCenter[2]),
                                                            RotationalInertia((*robotJoints)[i].inertiaMatrix[0],(*robotJoints)[i].inertiaMatrix[1],
                                                                        (*robotJoints)[i].inertiaMatrix[2],(*robotJoints)[i].inertiaMatrix[3],
                                                                        (*robotJoints)[i].inertiaMatrix[4],(*robotJoints)[i].inertiaMatrix[5]));
        if (((*robotJoints)[i].jointType==0)&&((*robotJoints)[i].dhType==1))//旋转关节，modified DH
        {
            dhFrame = Frame::DH_Craig1989(a, alpha, d, theta);
            robotChain.addSegment(Segment(Joint(Joint::RotZ), dhFrame,
                                   dhFrame.Inverse()*rigidBodyInertia));
        }
        else if (((*robotJoints)[i].jointType==1)&&((*robotJoints)[i].dhType==1))//移动关节，modified DH
        {
            dhFrame = Frame::DH_Craig1989(a, alpha, d, theta);
            robotChain.addSegment(Segment(Joint(Joint::TransZ), dhFrame,
                                   dhFrame.Inverse()*rigidBodyInertia));
        }
        else if (((*robotJoints)[i].jointType==0)&&((*robotJoints)[i].dhType==0))//旋转关节，standard DH
        {
            dhFrame = Frame::DH(a, alpha, d, theta);
            robotChain.addSegment(Segment(Joint(Joint::RotZ), dhFrame,
                                   rigidBodyInertia));
        }
        else//((robotJoints[i].jointType==1)&&(robotJoints[i].dhType==0))//移动关节，standard DH
        {
            dhFrame = Frame::DH(a, alpha, d, theta);
            robotChain.addSegment(Segment(Joint(Joint::TransZ), dhFrame,
                                   rigidBodyInertia));
        }
    }
    if(robotType == ENUM_6AXIS_CHAIN)
    {
        sixJointRobot = new SixJointRobot;
        sixJointRobotForThread2 = new SixJointRobot;

        //初始化ＤＨ参数
        float tmpDhParameter[6][4];
        for(int i=0;i<6;i++)
        {
            for(int j=0;j<4;j++)
            {
               tmpDhParameter[i][j]= (*robotJoints)[i].dhParameter[j];
            }
        }
        sixJointRobot->initialForIk(tmpDhParameter);
        sixJointRobotForThread2->initialForIk(tmpDhParameter);
    }
}

Rotation Coordinate::TaitBryanXYZ(double psi, double sita, double fai)
{
    double s1 =sin(psi);
    double c1 =cos(psi);
    double s2 =sin(sita);
    double c2 =cos(sita);
    double s3 =sin(fai);
    double c3 =cos(fai);
    return Rotation(c2*c3, -c2*s3, s2,
                    c1*s3+c3*s1*s2, c1*c3-s1*s2*s3, -c2*s1,
                    s1*s3-c1*c3*s2, c3*s1+c1*s2*s3, c1*c2);
}

void Coordinate::getTaitBryanXYZ(const Rotation &rotMat, double &psi, double &sita, double &fai)
{
//    double eps = 1E-12;
    fai = atan2(-rotMat.data[1], rotMat.data[0]);//范围-pi~pi
    sita = asin(rotMat.data[2]);//范围-pi/2~pi/2
    psi = atan2(-rotMat.data[5], rotMat.data[8]);//范围-pi~pi
}

int Coordinate::setToolCoordinateBy6Points(ToolCoordinateInfo& initInfo)
{
    //检查点的数量
    if(initInfo.pointList.size()<6){//点数量不足6个
        if(IF_DEBUG)
        {
            qDebug()<<"Coordinate::setToolCoordinateBy6Points====点数量不足" << initInfo.pointList.size();
        }
        return -1;
    }
    //检查点的格式
    for(int i=0; i<6; i++)//关节数目是否准确
    {
        if( (initInfo.pointList[i].jointFlag==1)||
                (internalNum != initInfo.pointList[i].positionValue.size()) ){//不是关节点或关节数目不正确
            if(IF_DEBUG)
            {
                qDebug()<<"Coordinate::setToolCoordinateBy6Points====不是关节点或关节数目不正确" << i <<
                          initInfo.pointList[i].jointFlag << initInfo.pointList[i].positionValue.size();
            }
            return -2;
        }
    }

    //前三个点设定工具坐标系原点
    Frame f1 = pointProToFrame(initInfo.pointList[0]);
    Frame f2 = pointProToFrame(initInfo.pointList[1]);
    Frame f3 = pointProToFrame(initInfo.pointList[2]);
    Vector v1;
    if(getPointBy3Mat(f1, f2, f3, v1) < 0)//不能得到工具坐标原点
    {
        if(IF_DEBUG)
        {
            qDebug() << "Coordinate::setToolCoordinateBy6Points====不能得到工具坐标原点";
        }
        return -3;
    }

    //后三个点设定旋转
    Frame f4 = pointProToFrame(initInfo.pointList[3]);
    Frame f5 = pointProToFrame(initInfo.pointList[4]);
    Frame f6 = pointProToFrame(initInfo.pointList[5]);
    Matrix3f rf3;
    f4.p = f4.M * v1 + f4.p;
    f5.p = f5.M * v1 + f5.p;
    f6.p = f6.M * v1 + f6.p;
    rf3 << f4.p.data[0], f5.p.data[0], f6.p.data[0],
               f4.p.data[1], f5.p.data[1], f6.p.data[1],
               f4.p.data[2], f5.p.data[2], f6.p.data[2];
    Rotation rotMat;
    if(getRotationBy3Points(rf3, rotMat) < 0)//不能得到旋转矩阵
    {
        if(IF_DEBUG)
        {
            qDebug() << "Coordinate::setToolCoordinateBy6Points====不能得到旋转矩阵";
        }
        return -4;
    }

    initInfo.forwardTcpMatrix.M =  rotMat;//f4.M.Inverse() *
    initInfo.forwardTcpMatrix.p = v1;
//    if(IF_DEBUG)
//    {
//        qDebug() << "Coordinate::setToolCoordinateBy6Points====rotMat" << rotMat.data[0] << rotMat.data[1] << rotMat.data[2] << rotMat.data[3] << rotMat.data[4] <<
//                    rotMat.data[5] << rotMat.data[6] << rotMat.data[7] << rotMat.data[8];
//        double x, y, z;
//        getTaitBryanXYZ(rotMat, x,y,z);
//        qDebug() << "Coordinate::setToolCoordinateBy6Points====rotMat" << x << y << z;
//    }
    PointPro xyz = frameToPointPro_CART(initInfo.forwardTcpMatrix);
    initInfo.xyzwpr = QVector<float>::fromStdVector( xyz.positionValue );
    initInfo.forwardTcpMatrix = pointProToFrame(xyz);
    initInfo.inverseTcpMatrix = initInfo.forwardTcpMatrix.Inverse();
    initInfo.error = 0.0;
    Vector o1 = f1.p + f1.M  * v1;
    Vector o2 = f2.p + f2.M  * v1;
    Vector o3 = f3.p + f3.M  * v1;
    v1 = o1 - o2;
    double d = sqrt(v1.data[0] * v1.data[0] + v1.data[1] * v1.data[1] + v1.data[2] * v1.data[2]) * 1000.0;
    if(d>initInfo.error){
        initInfo.error = d;
    }
    v1 = o2 - o3;
    d = sqrt(v1.data[0] * v1.data[0] + v1.data[1] * v1.data[1] + v1.data[2] * v1.data[2]) * 1000.0;
    if(d>initInfo.error){
        initInfo.error = d;
    }
    return 1;
}

int Coordinate::setToolCoordinateBy3Points(ToolCoordinateInfo &initInfo)
{
    //检查点的数量
    if(initInfo.pointList.size()<3){//点数量不足3个
        if(IF_DEBUG)
        {
            qDebug()<<"Coordinate::setToolCoordinateBy3Points====点数量不足" << initInfo.pointList.size();
        }
        return -1;
    }
    //检查点的格式
    for(int i=0; i<3; i++)//关节数目是否准确
    {
        if( (initInfo.pointList[i].jointFlag==1)||
                (internalNum != initInfo.pointList[i].positionValue.size()) ){//不是关节点或关节数目不正确
            if(IF_DEBUG)
            {
                qDebug()<<"Coordinate::setToolCoordinateBy3Points====不是关节点或关节数目不正确" << i <<
                          initInfo.pointList[i].jointFlag << initInfo.pointList[i].positionValue.size();
            }
            return -2;
        }
    }

    //三个点设定工具坐标系原点
    Frame f1 = pointProToFrame(initInfo.pointList[0]);
    Frame f2 = pointProToFrame(initInfo.pointList[1]);
    Frame f3 = pointProToFrame(initInfo.pointList[2]);
    Vector v1;
    if(getPointBy3Mat(f1, f2, f3, v1) < 0)//不能得到工具坐标原点
    {
        if(IF_DEBUG)
        {
            qDebug() << "Coordinate::setToolCoordinateBy3Points====不能得到工具坐标原点";
        }
        return -4;
    }

    initInfo.forwardTcpMatrix.M = Rotation::Identity();
    initInfo.inverseTcpMatrix.M = Rotation::Identity();
    initInfo.forwardTcpMatrix.p = v1;
    initInfo.inverseTcpMatrix.p = -v1;
    PointPro xyz = frameToPointPro_CART(initInfo.forwardTcpMatrix);
    initInfo.xyzwpr =QVector<float>::fromStdVector( xyz.positionValue );
    initInfo.error = 0.0;
    Vector o1 = f1.p + f1.M  * v1;
    Vector o2 = f2.p + f2.M  * v1;
    Vector o3 = f3.p + f3.M  * v1;
    v1 = o1 - o2;
    double d = sqrt(v1.data[0] * v1.data[0] + v1.data[1] * v1.data[1] + v1.data[2] * v1.data[2]) * 1000.0;
    if(d>initInfo.error){
        initInfo.error = d;
    }
    v1 = o2 - o3;
    d = sqrt(v1.data[0] * v1.data[0] + v1.data[1] * v1.data[1] + v1.data[2] * v1.data[2]) * 1000.0;
    if(d>initInfo.error){
        initInfo.error = d;
    }
    return 1;
}

int Coordinate::setToolCoordinateByXyzwpr(ToolCoordinateInfo &initInfo)
{
    //直接法使用世界坐标
    PointPro tmp;
    tmp.jointFlag =1;
    tmp.ufValue = 0;
    tmp.utValue = 0;
    tmp.positionValue.resize(6);
    tmp.positionValue = initInfo.xyzwpr.toStdVector();
    initInfo.forwardTcpMatrix = pointProToFrame(tmp);
    initInfo.inverseTcpMatrix = initInfo.forwardTcpMatrix.Inverse();
    initInfo.error = 0.0;
    return 1;
}

int Coordinate::setUserCoordinateBy3Points(UserCoordinateInfo& initInfo, const Frame& tcpForward)
{
    //检查点的数量
    if(initInfo.pointList.size()<3)//点数量不足3个
    {
        if(IF_DEBUG)
        {
            qDebug()<<"Coordinate::setUserCoordinateBy3Points====点数量不足3个"<<initInfo.pointList.size();
        }
        return -1;
    }
    //检查点的格式，1个点是否有3个数
    for(int i=0; i<3; i++)
    {
        if((initInfo.pointList[i].jointFlag==1)||
                (internalNum != initInfo.pointList[i].positionValue.size()))//点格式不对
        {
            if(IF_DEBUG)
            {
                qDebug()<<"Coordinate::setUserCoordinateBy3Points====点格式不对"<< i << initInfo.pointList[i].jointFlag << initInfo.pointList[i].positionValue.size();
            }
            return -2;
        }
    }
    Frame f1 = pointProToFrame(initInfo.pointList[0]) * tcpForward;
    Frame f2 = pointProToFrame(initInfo.pointList[1]) * tcpForward;
    Frame f3 = pointProToFrame(initInfo.pointList[2]) * tcpForward;
    Matrix3f rf3;
    rf3 << f1.p.data[0], f2.p.data[0], f3.p.data[0],
               f1.p.data[1], f2.p.data[1], f3.p.data[1],
               f1.p.data[2], f2.p.data[2], f3.p.data[2];
    Rotation rotMat;
    if(getRotationBy3Points(rf3, rotMat) < 0){//不能得到旋转矩阵
        if(IF_DEBUG)
        {
            qDebug()<<"Coordinate::setUserCoordinateBy3Points====不能得到旋转矩阵";
        }
        return -3;
    }
    initInfo.forwardTcpMatrix.M = rotMat;
    initInfo.forwardTcpMatrix.p = Vector(f1.p.data[0], f1.p.data[1], f1.p.data[2]);
    PointPro xyz = frameToPointPro_CART(initInfo.forwardTcpMatrix);
    initInfo.xyzwpr = QVector<float>::fromStdVector( xyz.positionValue );
    initInfo.forwardTcpMatrix = pointProToFrame(xyz);
    initInfo.inverseTcpMatrix = initInfo.forwardTcpMatrix.Inverse();
    return 1;
}

int Coordinate::setUserCoordinateBy4Points(UserCoordinateInfo &initInfo, const Frame& tcpForward)
{
    //检查点的数量
    if(initInfo.pointList.size()<4)//点数量不足3个
    {
        if(IF_DEBUG)
        {
            qDebug()<<"Coordinate::setUserCoordinateBy3Points====点数量不足3个"<<initInfo.pointList.size();
        }
        return -1;
    }
    //检查点的格式，1个点是否有3个数
    for(int i=0; i<4; i++)
    {
        if((initInfo.pointList[i].jointFlag==1)||
                (internalNum != initInfo.pointList[i].positionValue.size()))//点格式不对
        {
            if(IF_DEBUG)
            {
                qDebug()<<"Coordinate::setUserCoordinateBy3Points====点格式不对"<< i << initInfo.pointList[i].jointFlag << initInfo.pointList[i].positionValue.size();
            }
            return -2;
        }
    }
    Frame f1 = pointProToFrame(initInfo.pointList[0]) * tcpForward;
    Frame f2 = pointProToFrame(initInfo.pointList[1]) * tcpForward;
    Frame f3 = pointProToFrame(initInfo.pointList[2]) * tcpForward;
    Frame f0 = pointProToFrame(initInfo.pointList[3]) * tcpForward;
    Matrix3f rf3;
    rf3 << f1.p.data[0], f2.p.data[0], f3.p.data[0],
               f1.p.data[1], f2.p.data[1], f3.p.data[1],
               f1.p.data[2], f2.p.data[2], f3.p.data[2];
    Rotation rotMat;
    if(getRotationBy3Points(rf3, rotMat) < 0){//不能得到旋转矩阵
        if(IF_DEBUG)
        {
            qDebug()<<"Coordinate::setUserCoordinateBy3Points====不能得到旋转矩阵";
        }
        return -3;
    }
    initInfo.forwardTcpMatrix.M = rotMat;
    initInfo.forwardTcpMatrix.p = Vector(f0.p.data[0], f0.p.data[1], f0.p.data[2]);
    PointPro xyz = frameToPointPro_CART(initInfo.forwardTcpMatrix);
    initInfo.xyzwpr = QVector<float>::fromStdVector( xyz.positionValue );
    initInfo.forwardTcpMatrix = pointProToFrame(xyz);
    initInfo.inverseTcpMatrix = initInfo.forwardTcpMatrix.Inverse();
    return 1;
}

int Coordinate::setUserCoordinateByXyzwpr(UserCoordinateInfo &initInfo)
{
    //直接法使用世界坐标
    PointPro tmp;
    tmp.jointFlag =1;
    tmp.ufValue = 0;
    tmp.utValue = 0;
    tmp.positionValue.resize(6);
    tmp.positionValue = initInfo.xyzwpr.toStdVector();
    initInfo.forwardTcpMatrix = pointProToFrame(tmp);
    initInfo.inverseTcpMatrix = initInfo.forwardTcpMatrix.Inverse();
    return 1;
}

int Coordinate::getRotationBy3Points(const Matrix3f& rf3, Rotation& rotMat)
{
    MatrixXf Rf1(3, 3), vo(3, 3);
    RowVector3f ColNorm;
    Vector3f v1, v2, v3, nv1, nv2, nv3;

    v1 = rf3.col(1) - rf3.col(0);
    v2 = rf3.col(2) - rf3.col(0);
    v3 = v1.cross(v2);

    vo.col(0) = v1;
    vo.col(1) = v2;
    vo.col(2) = v3;

    if (fabs(vo.determinant()) < 1e-7){
        return -1;
    }
    Rf1 = getSchmidtMatrix(vo);
    ColNorm = Rf1.colwise().norm();
    nv1 = Rf1.col(0) / ColNorm[0];
    nv2 = Rf1.col(1) / ColNorm[1];
    nv3 = Rf1.col(2) / ColNorm[2];

    rotMat =Rotation(nv1(0), nv2(0), nv3(0),
                                   nv1(1), nv2(1), nv3(1),
                                   nv1(2), nv2(2), nv3(2));

    return 1;
}

MatrixXf Coordinate::getSchmidtMatrix(const MatrixXf& Sd)
{
    int n = Sd.rows();
    int m = Sd.cols();
    MatrixXf Orth(n, m);
    Orth.col(0) = Sd.col(0);
    MatrixXf DeltOrth(n, 1), TempM(n, 1);
    for (int i = 1; i != m; ++i)
    {
        DeltOrth << MatrixXf::Zero(n, 1);
        for (int j = 0; j != i; ++j)
        {
            float aa = Orth.col(j).transpose()*Sd.col(i);
            float bb = Orth.col(j).transpose()*Orth.col(j);
            TempM = aa / bb*Orth.col(j);
            //DeltOrth.col(0) = DeltOrth.col(0) + (Orth.col(j).transpose()*Sd.col(i) )/ (Orth.col(j).transpose()*Orth.col(j))*Orth.col(j);
            DeltOrth.col(0) = DeltOrth.col(0) + TempM.col(0);
        }
        Orth.col(i) = Sd.col(i) - DeltOrth.col(0);
    }
    return Orth;
}

int Coordinate::getPointBy3Mat(const Frame& T1, const Frame& T2, const Frame& T3, Vector& point)
{
    Matrix3f deltaRr1, deltaRr2, deltaRr3, A;
    MatrixXf deltaR(9, 3), deltaE(9, 1), deltaRtranspose(3, 9), b(3, 1), fresult(4, 1);
    Vector3f deltaEr1, deltaEr2, deltaEr3;

    //deltaRr1 = T2.block(0, 0, 3, 3) - T1.block(0, 0, 3, 3);
    deltaRr1 << T2.M.data[0]-T1.M.data[0], T2.M.data[1]-T1.M.data[1], T2.M.data[2]-T1.M.data[2],
                         T2.M.data[3]-T1.M.data[3], T2.M.data[4]-T1.M.data[4], T2.M.data[5]-T1.M.data[5],
                         T2.M.data[6]-T1.M.data[6], T2.M.data[7]-T1.M.data[7], T2.M.data[8]-T1.M.data[8];
    //deltaRr2 = T3.block(0, 0, 3, 3) - T2.block(0, 0, 3, 3);
    deltaRr2 << T3.M.data[0]-T2.M.data[0], T3.M.data[1]-T2.M.data[1], T3.M.data[2]-T2.M.data[2],
                         T3.M.data[3]-T2.M.data[3], T3.M.data[4]-T2.M.data[4], T3.M.data[5]-T2.M.data[5],
                         T3.M.data[6]-T2.M.data[6], T3.M.data[7]-T2.M.data[7], T3.M.data[8]-T2.M.data[8];
    //deltaRr3 = T3.block(0, 0, 3, 3) - T1.block(0, 0, 3, 3);
    deltaRr3 << T3.M.data[0]-T1.M.data[0], T3.M.data[1]-T1.M.data[1], T3.M.data[2]-T1.M.data[2],
                         T3.M.data[3]-T1.M.data[3], T3.M.data[4]-T1.M.data[4], T3.M.data[5]-T1.M.data[5],
                         T3.M.data[6]-T1.M.data[6], T3.M.data[7]-T1.M.data[7], T3.M.data[8]-T1.M.data[8];
    deltaR << deltaRr1, deltaRr2, deltaRr3;

    //deltaEr1 = T1.block(0, 3, 3, 1) - T2.block(0, 3, 3, 1);
    deltaEr1 << T1.p.data[0]-T2.p.data[0], T1.p.data[1]-T2.p.data[1], T1.p.data[2]-T2.p.data[2];
    //deltaEr2 = T2.block(0, 3, 3, 1) - T3.block(0, 3, 3, 1);
    deltaEr2 << T2.p.data[0]-T3.p.data[0], T2.p.data[1]-T3.p.data[1], T2.p.data[2]-T3.p.data[2];
    //deltaEr3 = T1.block(0, 3, 3, 1) - T3.block(0, 3, 3, 1);
    deltaEr3 << T1.p.data[0]-T3.p.data[0], T1.p.data[1]-T3.p.data[1], T1.p.data[2]-T3.p.data[2];
    deltaE << deltaEr1, deltaEr2, deltaEr3;
    deltaRtranspose = deltaR.transpose();
    A = deltaRtranspose*deltaR;
    //cout << "deltaR:" << endl << deltaR << endl;
    //cout << "deltaE:" << endl << deltaE << endl;
    //cout << "A:" << endl <<A << endl;
    b = deltaRtranspose*deltaE;
    //cout << "b:" << endl << b<< endl;
    //bool invertible;
    //float determinant;

    //A.computeInverseAndDetWithCheck(Ainverse, determinant, invertible);
    //if (invertible)
    //{
    //	fresult.block(0,0,3,1)= Ainverse*b;
    //	//fresult.block(0, 0, 3, 1) = fresult.block(0, 0, 3, 1).lu().solve(b);
    //	fresult(3, 0) = 0;
    //}
    //else
    //{
    //	fresult << -1, -1, -1, -1;
    //}

    fresult.block(0, 0, 3, 1) = A.fullPivLu().solve(b);
    if (!(A*fresult.block(0, 0, 3, 1)).isApprox(b))
        return -1;
    point = Vector(fresult(0,0), fresult(1,0), fresult(2,0));
    return 1;
}

Frame Coordinate::pointProToFrame(const PointPro &pointPro)
{
    Frame f;
    if(pointPro.jointFlag==1)
    {
//        f = Frame(Rotation::EulerZYZ(pointPro.positionValue[3]*M_PI/180.0,
//                        pointPro.positionValue[4]*M_PI/180.0,
//                        pointPro.positionValue[5]*M_PI/180.0),
//                        Vector (pointPro.positionValue[0]/1000.0,
//                            pointPro.positionValue[1]/1000.0,
//                            pointPro.positionValue[2]/1000.0));
        f = Frame(TaitBryanXYZ(pointPro.positionValue[3]*M_PI/180.0,
                        pointPro.positionValue[4]*M_PI/180.0,
                        pointPro.positionValue[5]*M_PI/180.0),
                        Vector (pointPro.positionValue[0]/1000.0,
                            pointPro.positionValue[1]/1000.0,
                            pointPro.positionValue[2]/1000.0));
    }
    else
    {
        JntArray jointArray(internalNum);
        for(int i=0; i<internalNum; i++)
        {
            if((*robotJoints)[i].jointType==0)//旋转关节
            {
                jointArray(i) = pointPro.positionValue[i]*M_PI/180.0;
            }
            else//移动关节
            {
                jointArray(i) = pointPro.positionValue[i] / 1000.0;
            }
        }
        assert(getFkPos(jointArray,f)==0);
    }
    return f;
}

Frame Coordinate::jntArrayToFrame(const JntArray &jntArray)
{
    Frame f;
    assert(getFkPos(jntArray,f)==0);
    return f;
}

PointPro Coordinate::frameToPointPro_CART(const Frame &f, int ufValue, int utValue)
{
    PointPro pointOut;
    float v[6];
    double v3,v4,v5;
//    f.M.GetEulerZYZ(v3, v4, v5);
    getTaitBryanXYZ(f.M, v3, v4, v5);
    v[0] = f.p[0]*1000.0; //笛卡尔，三维坐标，m转mm
    v[1] = f.p[1]*1000.0;
    v[2] = f.p[2]*1000.0;
    v[3] = v3*180.0/M_PI;//笛卡尔，欧拉角，弧度转度
    v[4] = v4*180.0/M_PI;
    v[5] = v5*180.0/M_PI;
    pointOut.jointFlag = 1;
    pointOut.ufValue = ufValue;
    pointOut.utValue = utValue;
    pointOut.positionValue.resize(6);
    for(int i=0; i<6; i++)
    {
        pointOut.positionValue[i]=v[i];
    }
//    if(IF_DEBUG)
//    {
//        qDebug() << "Coordinate::frameToPointPro_CART====" << QVector<float>::fromStdVector(pointOut.positionValue);
//    }
    return pointOut;
}

int Coordinate::transConfigString2ConfigInt(QString configString, int fut[6])
{
    QString pattern("\\s*([FNLR]{1}),\\s*([U]?[D]?)\\s*,?\\s*([B]?[T]?)\\s*,?(\\-?)(\\d)\\s*,(\\-?)(\\d)\\s*,(\\-?)(\\d)\\s*,?(\\-?)(\\d?)\\s*,?(\\-?)(\\d?)\\s*,?(\\-?)(\\d?)\\s*,?(.*)");
    QRegExp rx(pattern);
    configString.indexOf(rx);
    QString capStr1 = rx.cap(1);
    QString capStr2 = rx.cap(2);
    QString capStr3 = rx.cap(3);
    QString capSymb1 = rx.cap(4);
    QString capNum1 = rx.cap(5);
    QString capSymb2 = rx.cap(6);
    QString capNum2 = rx.cap(7);
    QString capSymb3 = rx.cap(8);
    QString capNum3 = rx.cap(9);
    fut[3] = capNum1.toInt();
    fut[4] = capNum2.toInt();
    fut[5] = capNum3.toInt();
    if(capSymb1=="-")
    {
        fut[3] = -fut[3];
    }
    if(capSymb2=="-")
    {
        fut[4] = -fut[4];
    }
    if(capSymb3=="-")
    {
        fut[5] = -fut[5];
    }
    if (capStr1 == "F")
    {
        fut[0] = 1;
    }
    else
    {
        fut[0] = 0;
    }
    if (capStr2 == "U")
    {
        fut[1] = 1;
    }
    else
    {
        fut[1] = 0;
    }
    if (capStr3 == "T")
    {
        fut[2] = 1;
    }
    else
    {
        fut[2] = 0;
    }

}

int Coordinate::frameToPointPro_JOINT(const Frame& f,  PointPro& pointOut, int ufValue, int utValue)
{
    JntArray jointsOut(internalNum);
    JntArray jointsInit(internalNum);
    pointOut.configInt.resize(internalNum);
    QString str = QString::fromStdString(pointOut.configString);
    switch (robotType)
    {
        case ENUM_SCARA:
        {
            QString pattern("\\s*([FNLR]{1}),\\s*([U]?[D]?)\\s*,?\\s*([B]?[T]?)\\s*,?(\\-?)(\\d)\\s*,(\\-?)(\\d)\\s*,(\\-?)(\\d)\\s*,(\\-?)(\\d)\\s*,(\\-?)(\\d)\\s*,(\\-?)(\\d)\\s*,(.*)");
            QRegExp rx(pattern);
            str.indexOf(rx);
            QString capStr1 = rx.cap(1);
            QString capStr2 = rx.cap(2);
            QString capStr3 = rx.cap(3);
            QString capSymb = rx.cap(4);
            QString capNum = rx.cap(5);
            int capValue = capNum.toInt();
            if(capSymb=="-"){
                capValue = -capValue;
            }
            pointOut.configInt[3] = capValue;
            if(capStr1=="L"){
                jointsInit(1) = -M_PI_2;
            }
            else{
                jointsInit(1) = M_PI_2;
            }
            if(getIkPos_SCARA(jointsInit, f, jointsOut)<0)
            {
                if(IF_DEBUG)
                {
                    qDebug() << "Coordinate::frameToPointPro_JOINT====SCARA坐标转换失败";
                }
                return -2;
            }
            break;
        }
        case ENUM_6AXIS_CHAIN:
        {
            int fut[6];
            transConfigString2ConfigInt(str, fut);
            int ikPos = getIkPos_SIX_FUT(fut, f, jointsOut);
            if(ikPos < 0)
            {
                if(IF_DEBUG)
                {
                    qDebug() << "Coordinate::frameToPointPro_JOINT====坐标转换失败";
                }
                return ikPos;
            }
            break;
        }
//        case ENUM_4AXIS_PALLET:
//        {
//            break;
//        }
        default:
        {
            if(IF_DEBUG)
            {
                qDebug()<<"Coordinate::frameToPointPro_JOINT====程序bug,不支持机器人类型"<< robotType;
            }
            return -1;
        }
    }
    pointOut.jointFlag = 0;
    pointOut.ufValue = ufValue;
    pointOut.utValue = utValue;
    pointOut.positionValue.clear();
    pointOut.positionValue.resize(internalNum);
    pointOut.configString.clear();
    for(int i=0; i<internalNum; i++)
    {
        if(0 == (*robotJoints)[i].jointType){//旋转关节
            //2017-5-23  彭秋明修改
            if (robotType == ENUM_SCARA)
            {
                pointOut.positionValue[i] = jointsOut(i) * 180.0/M_PI + 360.0 * pointOut.configInt[i];//旋转关节，弧度转度
            }
            if (robotType == ENUM_6AXIS_CHAIN)
            {
                pointOut.positionValue[i] = jointsOut(i) * 180.0/M_PI;//旋转关节，弧度转度
            }
        }
        else{//移动关节
            pointOut.positionValue[i] = jointsOut(i) * 1000.0;//移动关节，m转mm
        }
    }
    pointOut.configInt.clear();
    return 1;
}

int Coordinate::getMultiConfig(const std::vector<float> positionValue, std::string &configString, std::vector<int> &configInt)
{
    switch(robotType)
    {
        case ENUM_SCARA:
        {
            configInt.resize(4);
            configInt[1] = 0;
            configInt[2] = 0;
            double tmp = positionValue[0] /360.0;
            if(tmp>0.0){
                configInt[0] = floor(tmp);//往下取整
            }
            else{
                configInt[0] = ceil(tmp);//往上取整
            }
            tmp = positionValue[3] /360.0;
            if(tmp>0.0){
                configInt[3] = floor(tmp);//往下取整
            }
            else{
                configInt[3] = ceil(tmp);//往上取整
            }
            QString tmpStr = QString::number(configInt[3])+",0,0,"+QString::number(configInt[0])+",0,0,";
            if(positionValue[1] < 0.0)//轴2判断多解
            {
                tmpStr = "L," +tmpStr;
            }
            else
            {
                tmpStr = "R," + tmpStr;
            }
            configString = tmpStr.toStdString();
            break;
        }
        case ENUM_6AXIS_CHAIN:
        {
            configInt.resize(6);
            //判断第一、四、六轴的多圈,支持-1,0,1三种圈数
            double tmp = positionValue[0] /360.0;
            if(tmp>0.0){
                configInt[3] = floor(tmp);//往下取整
            }
            else
            {
                configInt[3] = ceil(tmp);//往上取整
            }
            tmp = positionValue[3] /360.0;
            if(tmp>0.0){
                configInt[4] = floor(tmp);//往下取整
            }
            else{
                configInt[4] = ceil(tmp);//往上取整
            }
            tmp = positionValue[5] /360.0;
            if(tmp>0.0){
                configInt[5] = floor(tmp);//往下取整
            }
            else {
                configInt[5] = ceil(tmp);//往上取整
            }
            QString tmpStr = QString::number(configInt[3]) + "," +QString::number(configInt[4]) + "," +QString::number(configInt[5]) ;
            // 判断求解姿态,手腕上下F(N)，机臂上下Ｕ(D)，机臂前后T(B)
            int fut[3];
            float jointPosition[6];
            for (int i = 0; i < 6; i++)
            {
                jointPosition[i] = positionValue[i] / 180.0 * M_PI;
            }
            int ikConfig = sixJointRobotForThread2->getIkConfig(jointPosition, fut);
            if (-1 == ikConfig)
            {
                //addMsg(ENUM_MSG_ERROR,"Coordinate","Coordinate",7061,robotId);
                return -1;
            }
            if (-2 == ikConfig)
            {
                //addMsg(ENUM_MSG_ERROR,"Coordinate","Coordinate",7062,robotId);
                return -1;
            }
            if (-3 == ikConfig)
            {
               // addMsg(ENUM_MSG_ERROR,"Coordinate","Coordinate",7063,robotId);
                return -1;
            }
            for (int i = 0; i < 3; i++)
            {
                configInt[i] = fut[i];
            }
            if(configInt[2] == 0)//判断机臂前后T(B)
            {
                tmpStr = "B," +tmpStr;
            }
            else
            {
                tmpStr = "T," + tmpStr;
            }
            if(configInt[1] == 0)//判断机臂上下Ｕ(D)
            {
                tmpStr = "D," +tmpStr;
            }
            else
            {
                tmpStr = "U," + tmpStr;
            }
            if(configInt[0] == 0)//判断手腕上下F(N)
            {
                tmpStr = "N," +tmpStr;
            }
            else
            {
                tmpStr = "F," + tmpStr;
            }
            configString = tmpStr.toStdString();
            break;
        }
//        case ENUM_4AXIS_PALLET:
//        {
//            break;
//        }
        default:
        {
            if(IF_DEBUG)
            {
                qDebug()<<"Coordinate::getMultiConfig====程序bug,不支持机器人类型"<<robotType;
            }
            return -1;
        }
    }//end switch
    return 1;
}

int Coordinate::getFkPos(const JntArray& q_in, Frame& pos)
{
//    if(IF_DEBUG)
//    {
//        std::cout << robot->getNrOfJoints() << std::endl;
//    }
    return fkSolverPos->JntToCart(q_in, pos);
}

int Coordinate::getFkVel(const JntArrayVel& qvel_in, FrameVel& vel)
{
    return fkSolverVel->JntToCart(qvel_in, vel);
}

int Coordinate::getIkPos_NR_JL(const JntArray& q_init, const Frame& p_in, JntArray& joints)
{
//    joints = JntArray(robotChain->getNrOfJoints());
    return ikSolverPos_NR_JL->CartToJnt(q_init, p_in, joints);
}

int Coordinate::getIkPos_NR(const JntArray& q_init, const Frame& p_in, JntArray& joints)
{
//    joints = JntArray(robotChain->getNrOfJoints());
    return ikSolverPos_NR->CartToJnt(q_init, p_in, joints);
}

int Coordinate::getIkPos_LMA(const JntArray& q_init, const Frame& p_in, JntArray& joints)
{
//    joints = JntArray(robotChain->getNrOfJoints());
    return ikSolverPos_LMA->CartToJnt(q_init, p_in, joints);
}

int Coordinate::getIkVel_pinv(const JntArray& q_in, const Twist& v_in, JntArray& jointsDot)
{
//    jointsDot = JntArray(robotChain->getNrOfJoints());
    return ikSolverVel_pinv->CartToJnt(q_in, v_in, jointsDot);
}

int  Coordinate::getIkPos_quick(const JntArray& q_init, const Frame& p_in, JntArray& joints)
{
    switch(robotType)
    {
        case ENUM_SCARA:
        {
            return getIkPos_SCARA( q_init,  p_in,  joints);
        }
//        case ENUM_4AXIS_PALLET:
//        {
//            return ;
//        }
       case ENUM_6AXIS_CHAIN:
       {
            //debug　将q_init，p_in，joints写入文件
//            dataRecord->addItem();
//            for (int i = 0; i < 6; i++)
//            {
//                dataRecord->writeRecord(q_init(i));
//            }
//            for (int i = 0; i < 3; i++)
//            {
//                dataRecord->writeRecord(p_in.p.data[i]);
//            }
//            for (int i = 0; i < 6; i++)
//            {
//                dataRecord->writeRecord(joints(i));
//            }
           return getIkPos_SIX( q_init,  p_in,  joints);
        }
        default:
        {
            return -1;
        }
    }
}

#ifndef BETA
int Coordinate::getIkPos_SCARA(const JntArray& q_init, const Frame& p_in, JntArray& joints)
{
    //多组解临时变量
    double ikSolution[2][4];
    //目标矩阵格式转换，Frame -> Matrix4d
//    Matrix4d pose;
//    //x y z
//    pose(0,3)=p_in.p.data[0];
//    pose(1,3)=p_in.p.data[1];
//    pose(2,3)=p_in.p.data[2];
//    //旋转矩阵
//    pose(0,0)=p_in.M.data[0];
//    pose(0,1)=p_in.M.data[1];
//    pose(0,2)=p_in.M.data[2];
//    pose(1,0)=p_in.M.data[3];
//    pose(1,1)=p_in.M.data[4];
//    pose(1,2)=p_in.M.data[5];
//    pose(2,0)=p_in.M.data[6];
//    pose(2,1)=p_in.M.data[7];
//    pose(2,2)=p_in.M.data[8];

    //原来的DH[i].q=0 alpha=1 a=2 d=3 用( *robotJoints)[i].dhParameter[0]替换
    //几何解法的中间参数变量
    double x = p_in.p.data[0];//x坐标
    double y = p_in.p.data[1];//y坐标
    double r = sqrt(x*x + y*y);
    double a = ( *robotJoints)[0].dhParameter[0];//大臂长
    double b = ( *robotJoints)[1].dhParameter[0];//小臂长
    double angleRotation = atan2(y, x);
    double rad2 = p_in.p.data[2] - (*robotJoints)[0].dhParameter[2] - (*robotJoints)[1].dhParameter[2] - (*robotJoints)[3].dhParameter[2];//三轴实际位移量
    // rad2=(pose(2,3)-DH[0].d-DH[1].d-DH[3].d)/scaraSliderFactor*2*M_PI;

//    if(IF_DEBUG)
//    {
////        qDebug()<<"Coordinate::getIkPos_SCARA====q_init:";
////        for(int i=0; i<4; i++)
////        {
////            qDebug()<< i <<q_init(i);
////        }
//        std::cout<<"p_in"<<p_in<<std::endl;
//    }

    if ( (a + b < r)||
         (fabs(b - a) >= r))
    {
        if(IF_DEBUG)
        {
            qDebug() << "Coordinate::getIkPos_SCARA====位置超过工作空间，不可达"<<a<<b<<r;
        }
        return -1;//位置超过工作空间，不可达。
    }
    else if(a+b==r)
    {
        joints(0) = angleRotation;
        joints(1) = 0.0;
        joints(2) = rad2;
        joints(3) = acos( p_in.M.data[0] );
        return 0;
    }
    else
    {
        double angle1 = acos((a*a + r*r - b*b)/(2*a*r));//余弦定理
        double angle2 = acos((a*a + b*b - r*r)/(2*a*b));
        ikSolution[0][2]=rad2;
        ikSolution[1][2]=rad2;
        double y2;
        double p;
        p=atan2(-p_in.M.data[6],sqrt(p_in.M.data[0]*p_in.M.data[0]+p_in.M.data[2]*p_in.M.data[2]));
        y2=atan2(p_in.M.data[3]/cos(p),p_in.M.data[0]/cos(p));

        //左手解
        if ( angleRotation + angle1 > M_PI)
        {
            ikSolution[0][0]=angleRotation + angle1 - 2.0*M_PI;
        }else
        {
            ikSolution[0][0]=angleRotation + angle1;
        }
        ikSolution[0][1] = angle2 - M_PI;
        ikSolution[0][3] = y2 - ikSolution[0][0] - ikSolution[0][1];

        //右手解
        if (angleRotation - angle1 > -M_PI)
        {
            ikSolution[1][0]=angleRotation - angle1;
        }
        else
        {
            ikSolution[1][0]=angleRotation - angle1 + 2.0*M_PI;
        }
        ikSolution[1][1] = M_PI - angle2;
        ikSolution[1][3] = y2 - ikSolution[1][0] - ikSolution[1][1];
    }

    //旋转轴角度转换到－180～180
    for(int i=0; i<2; i++)
    {
          if(ikSolution[i][3] > M_PI)
          {
              ikSolution[i][3]=ikSolution[i][3] - 2*M_PI*ceil(ikSolution[i][3]/2.0/M_PI);
          }
          else if(ikSolution[i][3] <= -M_PI)
          {
              ikSolution[i][3]=ikSolution[i][3] - 2*M_PI*floor(ikSolution[i][3]/2.0/M_PI);
          }
          if((ikSolution[i][3] > M_PI)||(ikSolution[i][3] <= -M_PI))
          {
              qDebug()<<"Coordinate::getIkPos_SCARA===="<<ikSolution[i][3];
          }
//        ikSolution[i][3]=ikSolution[i][3] - 2.0*M_PI*round(ikSolution[i][3]/2.0/M_PI);
    }

    //选择最优解，左手，右手
    if(q_init(1)>0)//右手解
    {
        for(int k=0;k<4;k++)
        {
            joints(k)=ikSolution[1][k];
        }
    }
    else//左手解
    {
        for(int k=0;k<4;k++)
        {
            joints(k)=ikSolution[0][k];
        }
    }

    return 0;
}

#else
int Coordinate::getIkPos_SCARA(const JntArray& q_init, const Frame& p_in, JntArray& joints)
{
    //原来的DH[i].q=0 alpha=1 a=2 d=3 用( *robotJoints)[i].dhParameter[0]替换
    //几何解法的中间参数变量
    double eps = 1e-10;
    double x = p_in.p.data[0];//x坐标
    double y = p_in.p.data[1];//y坐标
    double r = sqrt(x*x + y*y);
    double a = ( *robotJoints)[0].dhParameter[0];//大臂长
    double b = ( *robotJoints)[1].dhParameter[0];//小臂长
    double angleRotation = atan2(y, x);
    double y2 = atan2(-p_in.M.data[1], p_in.M.data[0]);//r角
    joints(2)  = (p_in.p.data[2] - (*robotJoints)[0].dhParameter[2] - (*robotJoints)[1].dhParameter[2] - (*robotJoints)[3].dhParameter[2])
            /cos((*robotJoints)[2].dhParameter[2]);//三轴实际位移量

    if ( (a + b < r + eps)||
         (fabs(b - a) >= r))
    {
        if(IF_DEBUG)
        {
            qDebug() << "Coordinate::getIkPos_SCARA====位置超过工作空间，不可达"<<a<<b<<r;
        }
        //addMsg(ENUM_MSG_ERROR,"Coordinate","Coordinate",7060,robotId);
        return -1;//位置超过工作空间，不可达。
    }
    else if(eps >= fabs(a+b-r))
    {
        joints(0) = angleRotation;
        joints(1) = 0.0;
//        joints(3) = acos( p_in.M.data[0] );//acos 0~pi   atan2(-p_in.M.data[5], p_in.M.data[8])//范围-pi~pi
        joints(3) = y2 - angleRotation;
    }
    else
    {
        double angle1 = acos((a*a + r*r - b*b)/(2*a*r));//余弦定理0~pi
        double angle2 = acos((a*a + b*b - r*r)/(2*a*b));//余弦定理0~pi
        if( q_init(1)<0 )//左手
        {
            joints(1) = angle2 - M_PI;
            joints(0) = angleRotation + angle1;
            if(joints(0) > M_PI)
            {
                joints(0) = joints(0) - 2.0*M_PI;
            }
        }
        else//右手
        {
            joints(1) = M_PI - angle2;
            joints(0) = angleRotation - angle1;
            if(joints(0) < -M_PI)
            {
                joints(0) = joints(0) + 2.0*M_PI;
            }
        }
        joints(3) = y2 - joints(0) - joints(1);
    }
    if(joints(3) > M_PI)
    {
        joints(3) = joints(3) - 2*M_PI*ceil(joints(3)/2.0/M_PI);
    }
    else if(joints(3) <= -M_PI)
    {
        joints(3) = joints(3) - 2*M_PI*floor(joints(3)/2.0/M_PI);
    }
    if((joints(3) > M_PI)||(joints(3) <= -M_PI))
    {
        qDebug()<<"Coordinate::getIkPos_SCARA====ERROR"<<joints(3);
    }
    //多解多圈
    return 0;
}

int Coordinate::getIkPos_SIX_FUT(int fut[6], const Frame &p_in, JntArray &joints)
{
    //初始化中间变量
    float tmpPoseMatrix[4][4];
    float tmpJointSolution[6];

    tmpPoseMatrix[0][0]=p_in.M.data[0];
    tmpPoseMatrix[0][1]=p_in.M.data[1];
    tmpPoseMatrix[0][2]=p_in.M.data[2];
    tmpPoseMatrix[1][0]=p_in.M.data[3];
    tmpPoseMatrix[1][1]=p_in.M.data[4];
    tmpPoseMatrix[1][2]=p_in.M.data[5];
    tmpPoseMatrix[2][0]=p_in.M.data[6];
    tmpPoseMatrix[2][1]=p_in.M.data[7];
    tmpPoseMatrix[2][2]=p_in.M.data[8];

    tmpPoseMatrix[0][3]=p_in.p[0];
    tmpPoseMatrix[1][3]=p_in.p[1];
    tmpPoseMatrix[2][3]=p_in.p[2];
    tmpPoseMatrix[3][3]=1;
    tmpPoseMatrix[3][0]=0;
    tmpPoseMatrix[3][1]=0;
    tmpPoseMatrix[3][2]=0;

    //求解
    int isIK = sixJointRobotForThread2->getIkSolutionFUT(tmpPoseMatrix,fut,tmpJointSolution);
    if(1!=isIK)
    {
        qDebug()<<"get ik failed!";
        return isIK;
    }

    //求解值转换
    for(int i=0;i<6;i++)
    {
            joints(i)=tmpJointSolution[i];
    }

    return 0;
}

int Coordinate::getIkPos_SIX(const JntArray& q_init, const Frame& p_in, JntArray& joints)
{
    //初始化中间变量
    float tmpPoseMatrix[4][4];
    float tmpBeforeJoint[6];
    float tmpJointSolution[6];

    tmpPoseMatrix[0][0]=p_in.M.data[0];
    tmpPoseMatrix[0][1]=p_in.M.data[1];
    tmpPoseMatrix[0][2]=p_in.M.data[2];
    tmpPoseMatrix[1][0]=p_in.M.data[3];
    tmpPoseMatrix[1][1]=p_in.M.data[4];
    tmpPoseMatrix[1][2]=p_in.M.data[5];
    tmpPoseMatrix[2][0]=p_in.M.data[6];
    tmpPoseMatrix[2][1]=p_in.M.data[7];
    tmpPoseMatrix[2][2]=p_in.M.data[8];

    tmpPoseMatrix[0][3]=p_in.p[0];
    tmpPoseMatrix[1][3]=p_in.p[1];
    tmpPoseMatrix[2][3]=p_in.p[2];
    tmpPoseMatrix[3][3]=1;
    tmpPoseMatrix[3][0]=0;
    tmpPoseMatrix[3][1]=0;
    tmpPoseMatrix[3][2]=0;


    for(int j=0;j<6;j++)
    {
       tmpBeforeJoint[j]=q_init(j);
    }

    //求解
    int isIK = sixJointRobot->getIkSolution(tmpPoseMatrix,tmpBeforeJoint,tmpJointSolution);
    if(1!=isIK)
    {
        qDebug()<<"get ik failed!";
        return isIK;
    }

    //求解值转换
    for(int i=0;i<6;i++)
    {
            joints(i)=tmpJointSolution[i];
    }

    return 0;

}

void Coordinate::addMsg(int messageLevel, string componentName, string messageType, int messageCode, int _robotId,
                                     int parameter1, int parameter2, int parameter3, int parameter4)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = _robotId;

    string infomationStr;

    switch(messageCode)
    {

        case 7060://ENUM_MSG_ERROR
        {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(_robotId).toStdString() +
                    QObject::tr(")运动学位置逆解失败！").toStdString();
            infomationStr = str1;
            break;
        }
        case 7061://ENUM_MSG_ERROR
        {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(_robotId).toStdString() +
                    QObject::tr(")顶点奇异点逆解失败！").toStdString();
            infomationStr = str1;
            break;
         }
        case 7062://ENUM_MSG_ERROR
        {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(_robotId).toStdString() +
                    QObject::tr(")J3奇异点逆解失败！").toStdString();
            infomationStr = str1;
            break;
        }
        case 7063://ENUM_MSG_ERROR
        {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(_robotId).toStdString() +
                    QObject::tr(")J5奇异点逆解失败！").toStdString();
            infomationStr = str1;
            break;
        }
        default:
        {
            qDebug()<<"Coordinate::addMsg====not match error code";
            break;
        }
    }

    tmpMsg.MessageInformation = infomationStr;
    MessageLog::getInstance()->addMessage(tmpMsg);
}


#endif
